/**
 * @ Author: luoqi
 * @ Create Time: 2025-02-10 16:13
 * @ Modified by: luoqi
 * @ Modified time: 2025-02-11 21:14
 * @ Description:
 */

#ifndef _TRAJC_PLAN_H
#define _TRAJC_PLAN_H

#ifndef qfp_t 
typedef float qfp_t;
#endif

#ifndef TRAJC_NO_LIMIT
#define TRAJC_NO_LIMIT (0)
#endif

typedef struct {
    /* trajectory planning parameters limits */
    qfp_t vel_max;      // max velocity
    qfp_t acc_max;      // max acceleration
    qfp_t jerk_max;     // max jerk
    
    qfp_t x0;           // initial position
    qfp_t xt;           // target position
    qfp_t v0;           // initial velocity
    qfp_t vt;           // target velocity

    /* used to judge whether the time segment is currently in use */
    qfp_t t_ploop;      // position loop trajectory time    
    qfp_t t_vloop;      // velocity loop trajectory time
    qfp_t t_aloop;      // acceleration loop trajectory time
    
    /* time segment */
    qfp_t t1;       // time segment 1, acc = k * t
    qfp_t t2;       // time segment 2, acc = constant
    qfp_t t3;       // time segment 3, acc = -k * t
    qfp_t t4;       // time segment 4, acc = 0
    qfp_t t5;       // time segment 5, acc = -k * t
    qfp_t t6;       // time segment 6, acc = constant
    qfp_t t7;       // time segment 7, acc = k * t
    qfp_t t_total;  // total time
} TrajcSPlan;

int trajc_init(TrajcSPlan *trajc, qfp_t jerk_max, qfp_t acc_max, qfp_t vel_max);

/* update 7 segment s-curve trajectory planning */
int trajc_update(TrajcSPlan *trajc, qfp_t x0, qfp_t xt, qfp_t v0, qfp_t vt);

/* calculate target position */
qfp_t trajc_pos_calc(TrajcSPlan *trajc, qfp_t dt);

/* calculate target velocity */
qfp_t trajc_vel_calc(TrajcSPlan *trajc, qfp_t dt);

/* calculate target acceleration */
qfp_t trajc_acc_calc(TrajcSPlan *trajc, qfp_t dt);

#endif
